After discussions with Tridge, it makes sense for this to go into a separate library that can be used by both EKF2 and EKF3. The use by each estimator core should be controllable by a parameter enabling it to be set to off, ride-along, active (able to be used by resets) and any other use options.

The following graphs are from testing performed using a quadplane. This platform type enables both copter and fixed wing flight motion characteristics to be tested. For this test the number of filter banks was set to 5. A vertical takeoff was performed in QLOITER followed by a transition to horizontal flight in CRUISE.

GSF0.Y0 ... GSF0.Y4 show the yaw estimate from each of the 3-state EKF's. GSF0.YC is the composite yaw output from the Gaussian Sum Filter (GSF) which combines the yaw estimates from each 3-state EKF. For comparison the yaw estimate from the EKF3 is also shown. The GSF yaw starts to converge during the initial climb in QLOITER due to the small amount of horizontal motion and then rapidly finalises the convergence when the transition manouevre starts.

Here are the weights calculated by the GSF and used to combine the individual yaw outputs from the 3-state EKF's. Also plotted is the estimated 1-Sigma yaw uncertainty for the GSF output in degrees.

Here is the ground speed:

The centripetal acceleration compensation using airspeed appears to be working effectively with the GSF yaw estimate closely tracking the EKF3 yaw through fixed wing flight.

In addition to the flight test result presented above, another three flights with vertical takeoffs were performed with similar characteristics. The yaw partially converges during the vertical climb due to residual horizontal motion and rapidly completes convergence during transition to forward flight.

Takeoff 2

Takeoff 3

Takeoff 4

SITL test results for patch b947374 which eliminates the use of quaternions. This change was inspired by the old DCM algorithm.

Weights:

Yaw Estimates:

@tridge I've moved it into a separate class but still need to add it to EKF2.

Todays testing performed using a quadplane in both RW and FW flight modes:

A total of 5 RW->FW transitions were performed across two flights. The first flight was with only 1 instance of EKF3 running using IMU1 (EK3_IMU_MASK = 1), the second flight used two instances (EK3_IMU_MASK = 3)

Flight 1: https://drive.google.com/open?id=19oji0tqwpPHhY5Uya-NxSKO_tK0TsDOq
Flight 2:https://drive.google.com/open?id=1q0Sdy4bzHKcOLdZwnT_G4uncnOwfI0RG

The following plots shows a vertical takeoff with transition into forward flight from flight 2:

EKF estimates:

GSF Weights:

As per previous testing there is enough horizontal motion during the vertical climb to enable the GSF to correctly estimate the yaw immediately after takeoff although reported uncertainty for the GSF yaw estimate stays around close to radians until forward transition enables the estimate to be improved.

I think it was probably just the EKF3 failing GPS innovation checks. When the GPS is reporting good accuracy, it only takes a few metres of movement for the 5-sigma consistency check to fail.

I assume you ended up placing the vehicle at the takeoff location before applying power.

SITL Copter flyaway test of commit 19edca7

Weights:

Yaw:

Weights from test of b23f342

Thank you rmackay9 for posting the flight test video https://www.youtube.com/watch?v=3xW9hj-lxNU

I think we should consider adding the required IMU delay buffer into the yaw estimator and run a single yaw estimator on the first IMU instead of one per lane.

Comments other than this one have been addressed to the best of my knowledge. Moving this into a separate library with its own buffers for IMU and GPS is going to increase flash space unless we also move the IMU and GPs read and buffering into a template class to avoid another duplicate of the readGPSdata and readIMUdata functionality that is already replicated in EKF2 and EKF3. Doing this is beyond the time I have available for this task at this point in time and should be tackled in a separate PR.

Once this PR is in I can then then get the functionality into EKF2.

Testing at commit 6fc86ba using ArduPlane SITL used a takeoff with a 180 deg yaw error, tripping the yaw reset using the RC channel option and placing the vehicle into a loiter to monitor the centripetal accel correction. The airspeed sensor use was then stopped via the USE_ARSPEED parameter which forced the EKF to use the default value which is halfway between min and max param values. This resulted in a small increase in the velocity innovations for the 3-state filter banks, but yaw estimation remained acceptable.:

Edit: investigating the airspeed estimate received by the yaw estimator showed that there was a cm/s vs m/s error in ArduPlane.cpp where the default airspeed value was written. This has been fixed by 9e2abfa

See:

Today I was able to run multiple flight tests including the latest commits
ccd4490
and
f9d48b1 on a 250 size quad with COMPASS_AUTODEC = 0 and COMPASS_DEC = -1.59 (-90 deg).

Here is data from one of the flights - log here

Taking off in loiter mode, the EKF yaw reset 5m from the takeoff location and the fly-away arrested at 7m. The vehicle was recovered in RTL.

GPS ground track (yellow square is 10x10m)

Here are the yaw estimates - note the EKF3 yaw (yellow line) resets to the GSF estimate at 17:55:51 when the GSF's reported yaw uncertainty drops below 15 degrees which is the exisiting threshold requirement for the EKF3 to use the GSF yaw estimate.

Here are the GSF calculated weights for the individual yaw estimates:

Here are the EKF3 GPS velocity innovations - note the immediate reduction in levels after the yaw reset at 17:55:51

Here's the CPU and memory utilisation on the pixracer board with two EKF3 instances. Only the first EKF3 was running the GSF (EKF3_IMU_MASK = 3 and EK3_GSF_RUN = 1).

Note to self: remember to revert the changes to COMPASS_ORIENT before the next flight or your copter will come home in many pieces. :-)

Two small things:

• I'm not sure the PM.load is an accurate use of CPU but that's a side point
• we should squash the commits to remove the "Revert:" commits

Have squashed commit history and continued here: #13776